#include "geometry_msgs/msg/pose_array.hpp"
#include "rclcpp/rclcpp.hpp"

#include <algorithm>
#include <cmath>

class ShootNode : public rclcpp::Node {
public:
  ShootNode(const std::string &node_name) : Node(node_name) {
    RCLCPP_INFO(this->get_logger(), "Shoot Node '%s' has been started.",
                node_name.c_str());
    // 订阅装甲板位置数据话题
    armor_pose_subscription_ =
        this->create_subscription<geometry_msgs::msg::PoseArray>(
            "armor_pose_topic", 10,
            [&](const geometry_msgs::msg::PoseArray::SharedPtr msg) {
              armorPoseCallback(msg);
            });
  }

private:
  // 位置信息话题订阅者
  rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr
      armor_pose_subscription_;
  // 将 pose 的四元数转换为 rpy（弧度），并返回位置
  void poseToXYZRPY(const geometry_msgs::msg::Pose &pose, double &x, double &y,
                    double &z, double &roll, double &pitch, double &yaw) const;

  // 位置信息话题回调函数
  void armorPoseCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Received armor poses, count: %zu",
                msg->poses.size());
    for (size_t i = 0; i < msg->poses.size(); ++i) {
      const auto &pose = msg->poses[i];
      double x, y, z, roll, pitch, yaw;
      poseToXYZRPY(pose, x, y, z, roll, pitch, yaw);
      double roll_deg = roll * 180.0 / M_PI;
      double pitch_deg = pitch * 180.0 / M_PI;
      double yaw_deg = yaw * 180.0 / M_PI;
      RCLCPP_INFO(
          this->get_logger(),
          "Pose %zu: pos=(%.3f, %.3f, %.3f), rpy=(%.2f, %.2f, %.2f) deg", i, x,
          y, z, roll_deg, pitch_deg, yaw_deg);
    }
  }
};

// 将 pose 中的 quaternion 转为 rpy（弧度），并输出位置
void ShootNode::poseToXYZRPY(const geometry_msgs::msg::Pose &pose, double &x,
                             double &y, double &z, double &roll, double &pitch,
                             double &yaw) const {
  x = pose.position.x;
  y = pose.position.y;
  z = pose.position.z;

  // 四元数
  double qx = pose.orientation.x;
  double qy = pose.orientation.y;
  double qz = pose.orientation.z;
  double qw = pose.orientation.w;

  // 计算 roll (x-axis rotation)
  double sinr_cosp = 2.0 * (qw * qx + qy * qz);
  double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy);
  roll = std::atan2(sinr_cosp, cosr_cosp);

  // 计算 pitch (y-axis rotation)
  double sinp = 2.0 * (qw * qy - qz * qx);
  if (sinp >= 1.0)
    pitch = M_PI / 2.0; // use 90 degrees if out of range
  else if (sinp <= -1.0)
    pitch = -M_PI / 2.0;
  else
    pitch = std::asin(sinp);

  // 计算 yaw (z-axis rotation)
  double siny_cosp = 2.0 * (qw * qz + qx * qy);
  double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
  yaw = std::atan2(siny_cosp, cosy_cosp);
}

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ShootNode>("shoot_node");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}